
#include "misc.hpp"

// pin A1 1;
// pin A2 ?;
// from left bottom: D12,D11,...D2,GND
void setPin(String cmd)
{
	int pinIdx, val;
	if (cmd.length() > 0 && (cmd.charAt(0) == 'A' || cmd.charAt(0) == 'a'))
	{
		String pinIdxStr = cmd.substring(1, 2);
		pinIdx = myParseInt(pinIdxStr) + A0;
	}
	else
	{
		String pinIdxStr = cmd.substring(0, 2);
		pinIdx = myParseInt(pinIdxStr);
	}
	char qCheck = cmd.charAt(3);
	if (qCheck == '?')
	{
		//    pinMode(pinIdx, INPUT);
		val = digitalRead(pinIdx);
		flushReply("Read Pin {" + String(pinIdx) + "} as {" + String(val) + "};");
	}
	else
	{
		String valStr = cmd.substring(3);
		val = myParseInt(valStr);
		digitalWrite(pinIdx, val);
		flushReply("Set Pin {" + String(pinIdx) + "} to {" + String(val) + "};");
	}
}

char returnCmd[200];
int returnCmdUsed = 0;
void appendReply(String newReply)
{
	//  if(flagGlobalSilence){
	//    return;
	//  }
	// do not use long string
	// there is a limit on memory...and it will not be added
	if (newReply.length() > 0)
	{
		if (newReply.length() + returnCmdUsed < sizeof(returnCmd))
		{
			memcpy(returnCmd + returnCmdUsed, &newReply[0], newReply.length()); //是指在returnCmd的第returnCmdUsed个开始的位置ok
			returnCmdUsed += newReply.length();
		}
		else
		{
			Serial.println(F("Return command overflow"));
		}
	}
	else
	{
	}
}

void flushReply(String newReply)
{
	//  if(flagGlobalSilence){
	//    return;
	//  }
	const byte replyEnding[2] = { 0x0d, 0x0a };
	appendReply(newReply);
	if ((returnCmdUsed > 0) && (returnCmdUsed + 2 + 1 < sizeof(returnCmd)))
	{
		memcpy(returnCmd + returnCmdUsed, replyEnding, sizeof(replyEnding));
		returnCmdUsed += 2;
		returnCmd[returnCmdUsed] = '\0';
		Serial.write(returnCmd, returnCmdUsed);
		Serial.flush();
		returnCmdUsed = 0;
	}
}

void print2Serial(const char* content) {
	Serial.write(content);
	Serial.flush();
}

void flushSerialInput()
{
	while (Serial.available())
	{
		Serial.read();
	}
}

long myParseInt(String intStrIn)
{
	unsigned int startIdx, endIdx;
	for (startIdx = 0; startIdx < intStrIn.length(); startIdx++)
	{
		if (intStrIn[startIdx] != ' ')
		{
			break;
		}
	}
	for (endIdx = startIdx + 1; endIdx < intStrIn.length(); endIdx++)
	{
		if (intStrIn[endIdx] == ' ')
		{
			break;
		}
	}
	long withToInt = intStrIn.substring(startIdx, endIdx).toInt();
	return withToInt;
}

int myParsePin(String pinStrIn)
{
	if ((pinStrIn.charAt(0) == 'A' || pinStrIn.charAt(0) == 'a'))
	{
		String pinStrInLower = pinStrIn.substring(1, 2);
		return myParseInt(pinStrInLower) + A0;
	}
	else
	{
		return myParseInt(pinStrIn);
	}
}

String int2Str(int val, int padLen)
{
	char* buf = new char[padLen + 2];
	String format = String("%0") + String(padLen) + "d";
	sprintf(buf, format.c_str(), val);
	String ret = String(buf);

	delete[] buf;

	return ret;
}

String float2Str(float val, int digitNum)
{
	char* buf = new char[8 + 2 + digitNum + 2];
	String format = String("%.") + String(digitNum) + "f";
	sprintf(buf, format.c_str(), val);
	String ret = String(buf);

	delete[] buf;

	return ret;
}

String formatPinIdx(int pinIdx)
{
	if (pinIdx < 10)
	{
		return "0" + String(pinIdx);
	}
	else
	{
		return String(pinIdx);
	}
}

// seperated by ',', may end with a ','
void myParseInts(String intsStrIn, long** intsOut, int* intNum)
{
	*intNum = 1;
	for (int idx = 0; idx < intsStrIn.length(); idx++)
	{
		if (intsStrIn[idx] == ',' && idx != intsStrIn.length() - 1)
		{
			*intNum = *intNum + 1;
		}
	}
	long* intsOutTemp = new long[*intNum];

	int outPtr = 0;
	unsigned int startIdx = 0, endIdx = 0;
	//Serial.println("Full string: " + intsStrIn);
	while (startIdx < intsStrIn.length() && endIdx < intsStrIn.length())
	{
		if (intsStrIn[endIdx] == ',')
		{
			String newStr = intsStrIn.substring(startIdx, endIdx);
			//Serial.print("New Start: ");
			//Serial.println(startIdx);
			//Serial.print("New end: ");
			//Serial.println(endIdx);
			//Serial.println("New string: " + newStr);
			long withToInt = newStr.toInt();
			intsOutTemp[outPtr] = (int)withToInt;
			outPtr++;
			startIdx = endIdx + 1;
			endIdx = startIdx;
		}
		else
		{
			endIdx++;
		}
	}
	if (endIdx > startIdx)
	{
		long withToInt = intsStrIn.substring(startIdx, endIdx).toInt();
		intsOutTemp[outPtr] = withToInt;
	}

	*intsOut = intsOutTemp;
}

void myParseIntsNew(char* strIn, long* int0Out, long* int1Out)
{
	{
		long* ref = int0Out;
		if (ref == nullptr)
		{
			return;
		}
	}
}

void parseIntTest(String val)
{
	//  Serial.println("Parse int test");
	//  Serial.println("Input: {" + val + "}");
	//  Serial.println("Parsed as : {" + val + "}");
}

void printHelp()
{
	//  Serial.println("1. init dist sensors;");
	//  Serial.println("2. interruptable;");
	//  Serial.println("3. mov x 1024;");
	//  Serial.println("4. led 1 on;");
	//  Serial.println("5. read dist all 15;");
	//  Serial.println("6. read dist x 12;");
	//  Serial.println("7. cmd queue check;");
	//  Serial.println("8. stop;");
	//  Serial.println("9. hello;");
}

bool searchPara(String cmd, String para)
{
	return cmd.indexOf(para) >= 0;
}

bool searchPara(const char* cmd, const char* para) {
	return strstr(cmd, para) != nullptr;
}

void debug_parseInt(String strIn)
{
	Serial.print(F("Int Input: ["));
	Serial.println(strIn + "]");
	Serial.print(F("Parsed: ["));
	Serial.println(String(myParseInt(strIn)) + "]");
}

void debug_parseInts(String strIn)
{
	Serial.print(F("Ints Input: ["));
	Serial.println(strIn + "]");
	long* ints;
	int intNum;
	myParseInts(strIn, &ints, &intNum);
	Serial.print(F("Parsed "));
	Serial.print(String(intNum) + ":");
	for (int idx = 0; idx < intNum; idx++)
	{
		Serial.print("[" + String(ints[idx]) + "]");
	}

	delete[] ints;
	Serial.println("");
}

// bool StartWith(const char *a, const char *b)
// {
//   return (strncmp(a, b, strlen(b)) == 0);
// }